maze = [
    [1, 1, 1, 1, 1, 1, 1, 1, 1],
    [1, 0, 1, 1, 1, 0, 0, 1, 1],
    [1, 0, 1, 1, 0, 0, 0, 0, 1],
    [1, 0, 1, 1, 0, 1, 1, 0, 1],
    [1, 0, 0, 0, 0, 1, 1, 0, 1],
    [1, 1, 1, 0, 1, 1, 1, 1, 1],
    [1, 1, 1, 0, 1, 0, 0, 0, 1],
    [1, 1, 1, 0, 0, 0, 1, 0, 1],
    [1, 1, 1, 1, 1, 1, 1, 1, 1]
]
startx, starty = 1, 1
endx, endy = 4,4

dirs = [(0, 1), (1, 0), (0, -1), (-1, 0)]


def gs(i, j):  # 从起点到(i,j)的曼哈顿距离
    return abs(i - startx) + abs(j - starty)


def h1(i, j):
    return 10 * (abs(i - endx) + abs(j - endy))


def h2(i, j):
    return pow(i - endx, 2) + pow(j - endy, 2)


def h1_print_path(Close):
    path = []
    tmp = Close[-1]
    while tmp[4] != -1:
        path.append(Close[tmp[4]][0:2])
        tmp = Close[tmp[4]]
    path.reverse()
    print(f'A*算法(h1)得最短路径如下: ')
    for i in path:
        print(f'{i} → ', end='')
    print(f'{(endx, endy)}')
    print(f'长度为 {len(path)}, 总试探步数 {len(Close) - 1}')


def h2_print_path(Close):
    path = []
    tmp = Close[-1]
    while tmp[4] != -1:
        path.append(Close[tmp[4]][0:2])
        tmp = Close[tmp[4]]
    path.reverse()
    print(f'A*算法(h2)得最短路径如下: ')
    for i in path:
        print(f'{i} → ', end='')
    print(f'{(endx, endy)}')
    print(f'长度为 {len(path)}, 总试探步数 {len(Close) - 1}')


def AStar_h1():
    visited = [(startx, starty)]
    Open = [(startx, starty, 0, 0, -1)]  # 横纵坐标、f值、当前结点下标、父节点下标
    Close = [(startx, starty, 0, 0, -1)]
    while Open:
        Open.sort(key=lambda x: x[2], reverse=True)
        cur = Open.pop()
        for dir in dirs:
            tx = cur[0] + dir[0]
            ty = cur[1] + dir[1]
            if 0 < tx < 9 and 0 < ty < 9 and (tx, ty) not in visited and maze[tx][ty] == 0:
                f = gs(tx, ty) + h1(tx, ty)
                Open.append((tx, ty, f, len(Close), cur[3]))
                Close.append((tx, ty, f, len(Close), cur[3]))
                visited.append((tx, ty))
                if (tx, ty) == (endx, endy):
                    h1_print_path(Close)
                    return


def AStar_h2():
    visited = [(startx, starty)]
    Open = [(startx, starty, 0, 0, -1)]
    Close = [(startx, starty, 0, 0, -1)]
    # Close、Open中的节点是五元组: (横坐标、纵坐标、节点的静态估计值当前节点在Close中的下标、父节点在Close中的下标)
    while Open:
        Open.sort(key=lambda x: x[2], reverse=True)  # 将Open中结点按照静态估计值排列，出队静态估计值最小的节点
        cur = Open.pop()
        for dir in dirs:  # 向四个方向尝试
            tx = cur[0] + dir[0]
            ty = cur[1] + dir[1]
            # 下一步是否可走(不是边界、障碍且未被访问)
            if 0 < tx < 9 and 0 < ty < 9 and (tx, ty) not in visited and maze[tx][ty] == 0:
                f = gs(tx, ty) + h2(tx, ty)  # 计算静态估计值
                Open.append((tx, ty, f, len(Close), cur[3]))
                Close.append((tx, ty, f, len(Close), cur[3]))
                visited.append((tx, ty))
                if (tx, ty) == (endx, endy):  # 如果找到终点，直接输出最短路径，结束算法
                    h2_print_path(Close)
                    return


AStar_h1()
print()
AStar_h2()
